/*
 *  SID.cpp - 6581 emulation
 *
 *  SIDPlayer (C) 1996 Christian Bauer

 *
 * Incompatibilities:
 * ------------------
 *
 *  - Lots of empirically determined constants in the filter calculations,
 *    no notch filter
 *  - Voice 3 cannot be muted
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <MediaKit.h>

#include "SID.h"
#include "CPU.h"


// Constants
const ULONG SAMPLE_FREQ = 48000;	// Sample output frequency in Hz
const ULONG SID_FREQ = 985248;		// SID frequency in Hz
const ULONG SID_CYCLES = SID_FREQ/SAMPLE_FREQ;	// # of SID clocks per sample frame

// SID waveforms (some of them :-)
enum {
	WAVE_NONE,
	WAVE_TRI,
	WAVE_SAW,
	WAVE_TRISAW,
	WAVE_RECT,
	WAVE_TRIRECT,
	WAVE_SAWRECT,
	WAVE_TRISAWRECT,
	WAVE_NOISE
};

// EG states
enum {
	EG_IDLE,
	EG_ATTACK,
	EG_DECAY,
	EG_SUSTAIN,
	EG_RELEASE
};

// Filter types
enum {
	FILT_NONE,
	FILT_LP,
	FILT_BP,
	FILT_LPBP,
	FILT_HP,
	FILT_NOTCH,
	FILT_HPBP,
	FILT_ALL
};

// Voice 4 states
enum {
	V4_OFF,
	V4_GALWAY_NOISE,
	V4_SAMPLE
};


/*
 *  Static data
 */

UWORD MOS6581::TriTable[0x200];

const UWORD MOS6581::TriSawTable[0x100] = {
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0808,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1818, 0x3C3C,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x1C1C,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x8080, 0x0000, 0x8080, 0x8080,
	0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xE0E0,
	0xF0F0, 0xF0F0, 0xF0F0, 0xF0F0, 0xF8F8, 0xF8F8, 0xFCFC, 0xFEFE
};

const UWORD MOS6581::TriRectTable[0x100] = {
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0xFFFF, 0xFCFC, 0xF8F8, 0xF0F0, 0xF4F4, 0xF0F0, 0xF0F0, 0xE0E0,
	0xECEC, 0xE0E0, 0xE0E0, 0xC0C0, 0xE0E0, 0xC0C0, 0xC0C0, 0xC0C0,
	0xDCDC, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0x8080, 0x8080,
	0xC0C0, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0x0000, 0x0000,
	0xBEBE, 0xA0A0, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0x0000,
	0x8080, 0x8080, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x8080, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x7E7E, 0x7070, 0x6060, 0x0000, 0x4040, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000
};

const UWORD MOS6581::SawRectTable[0x100] = {
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8080,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8080, 0x8080,
	0x0000, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0xB0B0, 0xBEBE,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8080,
	0x0000, 0x0000, 0x0000, 0x8080, 0x8080, 0x8080, 0x8080, 0xC0C0,
	0x0000, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0xC0C0,
	0x8080, 0x8080, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xDCDC,
	0x8080, 0x8080, 0x8080, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0, 0xC0C0,
	0xC0C0, 0xC0C0, 0xC0C0, 0xE0E0, 0xE0E0, 0xE0E0, 0xE0E0, 0xECEC,
	0xC0C0, 0xE0E0, 0xE0E0, 0xE0E0, 0xE0E0, 0xF0F0, 0xF0F0, 0xF4F4,
	0xF0F0, 0xF0F0, 0xF8F8, 0xF8F8, 0xF8F8, 0xFCFC, 0xFEFE, 0xFFFF
};

const UWORD MOS6581::TriSawRectTable[0x100] = {
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8080, 0x8080,
	0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0x8080, 0xC0C0, 0xC0C0,
	0xC0C0, 0xC0C0, 0xE0E0, 0xE0E0, 0xE0E0, 0xF0F0, 0xF8F8, 0xFCFC
};

const ULONG MOS6581::EGTable[16] = {
	(SID_CYCLES << 16) / 9, (SID_CYCLES << 16) / 32,
	(SID_CYCLES << 16) / 63, (SID_CYCLES << 16) / 95,
	(SID_CYCLES << 16) / 149, (SID_CYCLES << 16) / 220,
	(SID_CYCLES << 16) / 267, (SID_CYCLES << 16) / 313,
	(SID_CYCLES << 16) / 392, (SID_CYCLES << 16) / 977,
	(SID_CYCLES << 16) / 1954, (SID_CYCLES << 16) / 3126,
	(SID_CYCLES << 16) / 3906, (SID_CYCLES << 16) / 11720,
	(SID_CYCLES << 16) / 19531, (SID_CYCLES << 16) / 31251
};

const UBYTE MOS6581::EGDRShift[256] = {
	5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,
	3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,
	2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
	2,2,2,2,2,2,2,2,1,1,1,1,1,1,1,1,
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};

const WORD MOS6581::SampleTab[3][16] = {
	0x8000, 0x9111, 0xa222, 0xb333, 0xc444, 0xd555, 0xe666, 0xf777,
	0x0888, 0x1999, 0x2aaa, 0x3bbb, 0x4ccc, 0x5ddd, 0x6eee, 0x7fff,

	0xc444, 0xc444, 0xd555, 0xd555, 0xe666, 0xe666, 0xf777, 0xf777,
	0x0888, 0x0888, 0x1999, 0x1999, 0x2aaa, 0x2aaa, 0x3bbb, 0x3bbb,

	0xe666, 0xe666, 0xe666, 0xe666, 0xf777, 0xf777, 0xf777, 0xf777,
	0x0888, 0x0888, 0x0888, 0x0888, 0x1999, 0x1999, 0x1999, 0x1999
};


/*
 *  Constructor; open subscriber, but do not enter stream yet
 */

MOS6581::MOS6581()
{
	// Link voices together
	voice[0].mod_by = &voice[2];
	voice[1].mod_by = &voice[0];
	voice[2].mod_by = &voice[1];
	voice[0].mod_to = &voice[1];
	voice[1].mod_to = &voice[2];
	voice[2].mod_to = &voice[0];

	// Calculate triangle table
	for (int i=0; i<0x100; i++) {
		TriTable[i] = (i << 8) | i;
		TriTable[0x1ff-i] = (i << 8) | i;
	}

	f_enabled = TRUE;
	Reset();

	in_stream = FALSE;
	the_sub = new BAudioSubscriber("6581 SID");
	ready = the_sub->Subscribe(B_DAC_STREAM, the_sub->ID(), FALSE) == B_NO_ERROR;
	if (ready) {
		the_sub->SetDACSampleInfo(2, 1, B_BIG_ENDIAN, B_LINEAR_SAMPLES);
		the_sub->SetSamplingRate(SAMPLE_FREQ);
		in_stream = FALSE;
	}
	SetStreamFreq(50);
}


/*
 *  Destructor
 */

MOS6581::~MOS6581()
{
	if (ready) {
		if (in_stream) {
			the_sub->ExitStream(TRUE);
			in_stream = FALSE;
		}
		the_sub->SetStreamBuffers(4096, 8);
		the_sub->Unsubscribe();
		ready = FALSE;
	}
	delete the_sub;
}


/*
 *  Reset the SID
 */

void MOS6581::Reset(void)
{
	volume = 15;
	v3_mute = FALSE;

	for (int i=0; i<0x80; i++)
		regs[i] = 0;

	for (int v=0; v<3; v++) {
		voice[v].wave = WAVE_NONE;
		voice[v].eg_state = EG_IDLE;
		voice[v].count = voice[v].add = 0;
		voice[v].freq = voice[v].pw = 0;
		voice[v].eg_level = voice[v].s_level = 0;
		voice[v].a_add = voice[v].d_sub = voice[v].r_sub = EGTable[0];
		voice[v].gate = voice[v].ring = voice[v].test = FALSE;
		voice[v].filter = voice[v].sync = FALSE;
	}

	f_type = FILT_NONE;
	f_freq = f_res = 0;
	f_ampl = 1.0;
	d1 = d2 = g1 = g2 = 0.0;
	xn1 = xn2 = yn1 = yn2 = 0.0;

	v4_state = V4_OFF;
	v4_count = v4_add = 0;
	gn_adr = gn_tone_length = 0;
	gn_volume_add = 0;
	gn_tone_counter = 0;
	gn_base_cycles = gn_loop_cycles = 0;
	sm_adr = sm_end_adr = sm_rep_adr = 0;
	sm_volume = 0;
	sm_rep_count = 0;
	sm_big_endian = FALSE;
}


/*
 *  Set stream frequency (= replay frequency)
 */

void MOS6581::SetStreamFreq(ULONG freq)
{
	stream_freq = freq;
}


/*
 *  Read from register
 */

UBYTE MOS6581::ReadRegister(UWORD adr)
{
	// A/D converters
	if (adr == 0x19 || adr == 0x1a) return 0xff;

	// Voice 3 oscillator/EG readout
	if (adr == 0x1b || adr == 0x1c) return rand() & 0xff;

	return 0;
}


/*
 *  Write to register
 */

void MOS6581::WriteRegister(UWORD adr, UBYTE byte)
{
	if (!ready)
		return;

	regs[adr] = byte;
	int v = adr/7;	// Voice number

	switch (adr) {
		case 0:
		case 7:
		case 14:
			voice[v].freq = (voice[v].freq & 0xff00) | byte;
			voice[v].add = (float)voice[v].freq * SID_FREQ / SAMPLE_FREQ;
			break;

		case 1:
		case 8:
		case 15:
			voice[v].freq = (voice[v].freq & 0xff) | (byte << 8);
			voice[v].add = (float)voice[v].freq * SID_FREQ / SAMPLE_FREQ;
			break;

		case 2:
		case 9:
		case 16:
			voice[v].pw = (voice[v].pw & 0x0f00) | byte;
			break;

		case 3:
		case 10:
		case 17:
			voice[v].pw = (voice[v].pw & 0xff) | ((byte & 0xf) << 8);
			break;

		case 4:
		case 11:
		case 18:
			voice[v].wave = (byte >> 4) & 0xf;
			if ((byte & 1) != voice[v].gate)
				if (byte & 1)	// Gate turned on
					voice[v].eg_state = EG_ATTACK;
				else			// Gate turned off
					if (voice[v].eg_state != EG_IDLE)
						voice[v].eg_state = EG_RELEASE;
			voice[v].gate = byte & 1;
			voice[v].mod_by->sync = byte & 2;
			voice[v].ring = byte & 4;
			if (voice[v].test = byte & 8)
				voice[v].count = 0;
			break;

		case 5:
		case 12:
		case 19:
			voice[v].a_add = EGTable[byte >> 4];
			voice[v].d_sub = EGTable[byte & 0xf];
			break;

		case 6:
		case 13:
		case 20:
			voice[v].s_level = (byte >> 4) * 0x111111;
			voice[v].r_sub = EGTable[byte & 0xf];
			break;

		case 22:
			if (byte != f_freq) {
				f_freq = byte;
				calc_filter();
			}
			break;

		case 23:
			voice[0].filter = byte & 1;
			voice[1].filter = byte & 2;
			voice[2].filter = byte & 4;
			if ((byte >> 4) != f_res) {
				f_res = byte >> 4;
				calc_filter();
			}
			break;

		case 24:
			volume = byte & 0xf;
			v3_mute = byte & 0x80;
			if (((byte >> 4) & 7) != f_type) {
				f_type = (byte >> 4) & 7;
				calc_filter();
			}
			break;

		case 29:
			if (byte) {
				if (byte < 0xfc) {			// Galway noise
					gn_adr = (regs[0x1f] << 8) | regs[0x1e];
					gn_tone_length = regs[0x3d];
					gn_volume_add = regs[0x3e] & 15;
					gn_tone_counter = byte;
					gn_base_cycles = regs[0x5d];
					gn_loop_cycles = regs[0x3f];
					v4_count = 0;
					v4_add = SID_CYCLES * 65536 / (TheRAM[gn_adr + gn_tone_counter] * gn_loop_cycles + gn_base_cycles);
					v4_state = V4_GALWAY_NOISE;

				} else if (byte == 0xfd) {	// Sample off
					v4_state = V4_OFF;

				} else {					// Sample on
					sm_adr = ((regs[0x1f] << 8) | regs[0x1e]) << 1;
					sm_end_adr = ((regs[0x3e] << 8) | regs[0x3d]) << 1;
					sm_rep_adr = ((regs[0x7f] << 8) | regs[0x7e]) << 1;
					sm_rep_count = regs[0x3f];
					sm_big_endian = regs[0x7d];
					switch (byte) {
						case 0xfc:
							sm_volume = 2;
							break;
						case 0xfe:
							sm_volume = 1;
							break;
						case 0xff:
							sm_volume = 0;
							break;
					};
					v4_count = 0;
					v4_add = SID_CYCLES * 65536 / ((regs[0x5e] << 8) | regs[0x5d]);
					v4_state = V4_SAMPLE;
				}
			} else
				v4_state = V4_OFF;			
			break;
	}
}


/*
 *  Enable/disable filters
 */

void MOS6581::EnableFilters(bool enable)
{
	f_enabled = enable;
	calc_filter();
}


/*
 *  Pause sound output
 */

void MOS6581::PauseSound(void)
{
	if (in_stream) {
		the_sub->ExitStream(TRUE);
		in_stream = FALSE;
	}
}


/*
 *  Resume sound output
 */

void MOS6581::ResumeSound(void)
{
	if (!in_stream) {
		the_sub->EnterStream(NULL, TRUE, this, stream_func, NULL, TRUE);
		the_sub->SetStreamBuffers(SAMPLE_FREQ / stream_freq * 2, 8);	// Must be called after EnterStream()
		in_stream = TRUE;
	}
}


/*
 *  Return pause state
 */

bool MOS6581::IsPaused(void)
{
	return !in_stream;
}


/*
 *  Calculate IIR filter coefficients
 */

void MOS6581::calc_filter(void)
{
	float fr, arg;

	// Check for some trivial cases
	if (f_type == FILT_NOTCH || f_type == FILT_ALL || !f_enabled) {
		d1 = 0.0; d2 = 0.0;
		g1 = 0.0; g2 = 0.0;
		f_ampl = 1.0;
		return;
	}
	if (f_type == FILT_NONE) {
		d1 = 0.0; d2 = 0.0;
		g1 = 0.0; g2 = 0.0;
		f_ampl = 0.0;
		return;
	}

	// Calculate resonance frequency
	if (f_type == FILT_LP)
		fr = 200.0 + 0.000384295 * f_freq * f_freq * f_freq + 0.2477575 * f_freq * f_freq - 7.5694444 * f_freq;
	else
		fr = 300.0 - 0.002133588 * f_freq * f_freq * f_freq + 0.8389559 * f_freq * f_freq - 20.770766 * f_freq;

	// Limit to <1/2 sample frequency
	arg = fr / (float)(SAMPLE_FREQ >> 1);
	if (arg > 0.99)
		arg = 0.99;
	if (arg < 0.01)
		arg = 0.01;

	// Calculate poles (resonance frequency and resonance)
	g2 = 0.55 + 1.2 * arg * arg - 1.2 * arg + (float)f_res * 0.0133333333;
	g1 = -2.0 * sqrt(g2) * cos(M_PI * arg);

	// Increase resonance if LP/HP combined with BP
	if (f_type == FILT_LPBP || f_type == FILT_HPBP)
		g2 += 0.1;

	// Stabilize filter
	if (fabs(g1) >= g2 + 1.0)
		if (g1 > 0.0)
			g1 = g2 + 0.99;
		else
			g1 = -(g2 + 0.99);

	// Calculate roots (filter characteristic) and input attenuation
	switch (f_type) {

		case FILT_LPBP:
		case FILT_LP:
			d1 = 2.0; d2 = 1.0;
			f_ampl = 0.25 * (1.0 + g1 + g2);
			break;

		case FILT_HPBP:
		case FILT_HP:
			d1 = -2.0; d2 = 1.0;
			f_ampl = 0.25 * (1.0 - g1 + g2);
			break;

		case FILT_BP:
			d1 = 0.0; d2 = -1.0;
			f_ampl = 0.25 * (1.0 + g1 + g2) * (1 + cos(M_PI * arg)) / (sin(M_PI * arg));
			break;

		default:
			break;
	}
}


/*
 *  Stream function 
 */

bool MOS6581::stream_func(void *arg, char *buf, long count)
{
	((MOS6581 *)arg)->calc_buffer((WORD *)buf, count);
	return TRUE;
}


/*
 *  Fill one audio buffer with calculated SID sound
 */

void MOS6581::calc_buffer(WORD *buf, long count)
{
	// Return if SetStreamBuffers() failed
	if (count != SAMPLE_FREQ/stream_freq * 2)
		return;

	// Execute 6510 play routine
	TheCPU->Emulate(PlayAdr, 0, 0, 0);

	// Get filter coefficients for faster access
	float cf_ampl = f_ampl;
	float cd1 = d1, cd2 = d2, cg1 = g1, cg2 = g2;

	// Calculate sound
	count >>= 1;	// 16 bit output, count is in bytes
	while (count--) {
		LONG sum_output = 0;
		LONG sum_output_filter = 0;

		// Loop for all three voices
		for (int j=0; j<3; j++) {
			DRVoice *v = &voice[j];

			// Envelope generators
			UWORD envelope;

			switch (v->eg_state) {
				case EG_ATTACK:
					v->eg_level += v->a_add;
					if (v->eg_level > 0xffffff) {
						v->eg_level = 0xffffff;
						v->eg_state = EG_DECAY;
					}
					break;
				case EG_DECAY:
					v->eg_level -= v->d_sub >> EGDRShift[v->eg_level >> 16];
					if (v->eg_level < v->s_level || v->eg_level > 0xffffff) {
						v->eg_level = v->s_level;
						v->eg_state = EG_SUSTAIN;
					}
					break;
				case EG_SUSTAIN:
					v->eg_level = v->s_level;
					break;
				case EG_RELEASE:
					v->eg_level -= v->r_sub >> EGDRShift[v->eg_level >> 16];
					if (v->eg_level > 0xffffff) {
						v->eg_level = 0;
						v->eg_state = EG_IDLE;
					}
					break;
				case EG_IDLE:
					v->eg_level = 0;
			}
			envelope = (v->eg_level * volume) >> 20;

			// Waveform generators
			UWORD output;

			if (!v->test)
				v->count += v->add;

			if (v->sync && (v->count > 0x1000000))
				v->mod_to->count = 0;

			v->count &= 0xffffff;

			switch (v->wave) {
				case WAVE_TRI:
					if (v->ring)
						output = TriTable[(v->count ^ (v->mod_by->count & 0x800000)) >> 15];
					else
						output = TriTable[v->count >> 15];
					break;
				case WAVE_SAW:
					output = v->count >> 8;
					break;
				case WAVE_RECT:
					if (v->count > (ULONG)(v->pw << 12))
						output = 0xffff;
					else
						output = 0;
					break;
				case WAVE_TRISAW:
					output = TriSawTable[v->count >> 16];
					break;
				case WAVE_TRIRECT:
					if (v->count > (ULONG)(v->pw << 12))
						output = TriRectTable[v->count >> 16];
					else
						output = 0;
					break;
				case WAVE_SAWRECT:
					if (v->count > (ULONG)(v->pw << 12))
						output = SawRectTable[v->count >> 16];
					else
						output = 0;
					break;
				case WAVE_TRISAWRECT:
					if (v->count > (ULONG)(v->pw << 12))
						output = TriSawRectTable[v->count >> 16];
					else
						output = 0;
					break;
				case WAVE_NOISE:
					if (v->count > 0x100000) {
						output = v->noise = (rand() & 0xff) << 8;
						v->count &= 0xfffff;
					} else
						output = v->noise;
					break;
				default:
					output = 0x8000;
			}
			if (v->filter)
				sum_output_filter += (WORD)(output ^ 0x8000) * envelope;
			else
				sum_output += (WORD)(output ^ 0x8000) * envelope;
		}

		// Galway noise/samples
		switch (v4_state) {

			case V4_GALWAY_NOISE:
				sum_output += (WORD)((((gn_volume_add * (v4_count >> 16)) & 0xf) * 0x1111) ^ 0x8000) << 8;
				v4_count += v4_add;
				if ((v4_count >> 16) > gn_tone_length) {
					if (gn_tone_counter) {
						gn_tone_counter--;
						v4_count &= 0xffff;
						v4_add = SID_CYCLES * 65536 / (TheRAM[gn_adr + gn_tone_counter] * gn_loop_cycles + gn_base_cycles);
					} else
						v4_state = V4_OFF;
				}
				break;

			case V4_SAMPLE:
				UBYTE sample = TheRAM[sm_adr >> 1] * 0x0101;
				if (sm_big_endian)
					if (sm_adr & 1)
						sample = sample & 0xf;
					else
						sample = sample >> 4;
				else
					if (sm_adr & 1)
						sample = sample >> 4;
					else
						sample = sample & 0xf;
				sum_output += SampleTab[sm_volume][sample] << 8;
				v4_count += v4_add;
				sm_adr += v4_count >> 16;
				v4_count &= 0xffff;
				if (sm_adr >= sm_end_adr) {
					if (sm_rep_count) {
						if (sm_rep_count != 0xff)
							sm_rep_count--;
						sm_adr = sm_rep_adr;
					} else
						v4_state = V4_OFF;
				}
				break;
		}

		// Filter
		float xn = (float)sum_output_filter * cf_ampl;
		float yn = xn + cd1 * xn1 + cd2 * xn2 - cg1 * yn1 - cg2 * yn2;
		yn2 = yn1; yn1 = yn; xn2 = xn1; xn1 = xn;
		sum_output_filter = (LONG)yn;

		*buf++ += (sum_output + sum_output_filter) >> 10;
	}
}
